package at.jku.pervasive.recognition;

import javax.vecmath.Point3d;
import javax.vecmath.Vector2d;

import at.jku.pervasive.Position;

public class GestureCharacteristics
{

	// +/- 40 degrees
	public static double ANGLE_TOLERANCE = 40.0;

	// +/- 25 centimeters
	public static double EQUAL_HEIGHT_TOLERANCE = 0.25;

	// height difference of at least 30 centimeters
	public static double LOWER_TOLERANCE = 0.3;

	private boolean angleIs0;
	private boolean angleIs180;
	private boolean angleIs90;
	private double angleXY;

	private final Position left;
	private final Position reference;
	private final Position right;

	private boolean leftHasEqualHeightToRight;
	private boolean leftIsLowerThanReference;
	private boolean leftIsLowerThanRight;

	private boolean referenceHasEqualHeightToLeft;
	private boolean referenceHasEqualHeightToRight;
	private boolean referenceIsLowerThanLeft;
	private boolean referenceIsLowerThanRight;

	private boolean rightIsLowerThanLeft;
	private boolean rightIsLowerThanReference;

	public GestureCharacteristics(Position left, Position reference, Position right)
	{
		this.left = left;
		this.reference = reference;
		this.right = right;

		calculateCharacteristics();
	}

	public boolean angleIs0()
	{
		return angleIs0;
	}

	public boolean angleIs180()
	{
		return angleIs180;
	}

	public boolean angleIs90()
	{
		return angleIs90;
	}

	private void calculateCharacteristics()
	{
		// convert position to points
		Point3d leftPoint = getPoint3d(left);
		Point3d refPoint = getPoint3d(reference);
		Point3d rightPoint = getPoint3d(right);

		// calculate angle between the vectors, which are given by the position
		// of the left and the right hand
		double[] planeAngles = calculatePlaneAngles(leftPoint, refPoint, rightPoint);
		angleXY = planeAngles[0];

		// extract characteristics
		leftIsLowerThanRight = isLower(leftPoint, rightPoint);
		leftIsLowerThanReference = isLower(leftPoint, refPoint);
		leftHasEqualHeightToRight = isEqualHeight(leftPoint, rightPoint);

		rightIsLowerThanLeft = isLower(rightPoint, leftPoint);
		rightIsLowerThanReference = isLower(rightPoint, refPoint);

		referenceIsLowerThanRight = isLower(refPoint, rightPoint);
		referenceIsLowerThanLeft = isLower(refPoint, leftPoint);
		referenceHasEqualHeightToRight = isEqualHeight(refPoint, rightPoint);
		referenceHasEqualHeightToLeft = isEqualHeight(leftPoint, refPoint);

		angleIs180 = isWithin180DegreeTolerance(angleXY);
		angleIs90 = isWithin90DegreeTolerance(angleXY);
		angleIs0 = isWithin0DegreeTolerance(angleXY);
	}

	/**
	 * Calculate the XY, XZ and YZ angles in degrees between the given points.
	 * @param leftPoint
	 * @param refPoint
	 * @param rightPoint
	 * @return array, containing the angles, where arr[0] => XY angle, arr[1] =>
	 *         XZ angle and arr[2] => YZ angle
	 */
	private double[] calculatePlaneAngles(Point3d leftPoint, Point3d refPoint, Point3d rightPoint)
	{
		// get angle in XY
		Vector2d refXY = new Vector2d(refPoint.x, refPoint.y);
		Vector2d leftXY = new Vector2d(leftPoint.x, leftPoint.y);
		leftXY.sub(refXY);
		Vector2d rightXY = new Vector2d(rightPoint.x, rightPoint.y);
		rightXY.sub(refXY);
		double angleXY = leftXY.angle(rightXY);
		double angleXYDegrees = Math.toDegrees(angleXY);

		// get angle in XZ
		Vector2d refXZ = new Vector2d(refPoint.x, refPoint.z);
		Vector2d leftXZ = new Vector2d(leftPoint.x, leftPoint.z);
		leftXZ.sub(refXZ);
		Vector2d rightXZ = new Vector2d(rightPoint.x, rightPoint.z);
		rightXZ.sub(refXZ);

		double angleXZ = leftXZ.angle(rightXZ);
		double angleXZDegrees = Math.toDegrees(angleXZ);

		// get angle in YZ
		Vector2d refYZ = new Vector2d(refPoint.y, refPoint.z);
		Vector2d leftYZ = new Vector2d(leftPoint.y, leftPoint.z);
		leftYZ.sub(refYZ);
		Vector2d rightYZ = new Vector2d(rightPoint.y, rightPoint.z);
		rightYZ.sub(refYZ);
		double angleYZ = leftYZ.angle(rightYZ);
		double angleYZDegrees = Math.toDegrees(angleYZ);

		// finally return the array with the calculates angles
		return new double[] { angleXYDegrees, angleXZDegrees, angleYZDegrees };
	}

	public double getAngleXY()
	{
		return angleXY;
	}

	protected Point3d getPoint3d(Position position)
	{
		Point3d point3d = new Point3d();
		point3d.x = position.getX();
		point3d.y = position.getY();
		point3d.z = position.getZ();
		return point3d;
	}

	/**
	 * Check if two points have the same height.
	 * @param first
	 * @param second
	 * @return
	 */
	private boolean isEqualHeight(Point3d first, Point3d second)
	{
		return Math.abs(first.z - second.z) <= EQUAL_HEIGHT_TOLERANCE;
	}

	/**
	 * Check if a point is lower than an other one.
	 * @param first
	 * @param second
	 * @return <code>true</code> if the z coordinate of the first point is
	 *         smaller than the z coordinate of the second; <code>false</code>
	 *         otherwise
	 */
	private boolean isLower(Point3d first, Point3d second)
	{
		return (first.z + LOWER_TOLERANCE) < second.z;
	}

	private boolean isWithin0DegreeTolerance(double angle)
	{
		return isWithinTolerance(0.0, angle);
	}

	private boolean isWithin180DegreeTolerance(double value)
	{
		return isWithinTolerance(180.0, value);
	}

	private boolean isWithin90DegreeTolerance(double value)
	{
		return isWithinTolerance(90.0, value);
	}

	private boolean isWithinTolerance(double mainValue, double value)
	{
		return Math.abs(mainValue - value) <= ANGLE_TOLERANCE;
	}

	public boolean leftHasEqualHeightToRight()
	{
		return leftHasEqualHeightToRight;
	}

	public boolean leftIsLowerThanRef()
	{
		return leftIsLowerThanReference;
	}

	public boolean leftIsLowerThanRight()
	{
		return leftIsLowerThanRight;
	}

	public boolean refHasEqualHeightToLeft()
	{
		return referenceHasEqualHeightToLeft;
	}

	public boolean refHasEqualHeightToRight()
	{
		return referenceHasEqualHeightToRight;
	}

	public boolean refIsLowerThanLeft()
	{
		return referenceIsLowerThanLeft;
	}

	public boolean refIsLowerThanRight()
	{
		return referenceIsLowerThanRight;
	}

	public boolean rightIsLowerThanLeft()
	{
		return rightIsLowerThanLeft;
	}

	public boolean rightIsLowerThanRef()
	{
		return rightIsLowerThanReference;
	}

}
